#include <pololu/3pi.h>
#include <math.h>
#include <stdlib.h>

// make ; avrdude -p m328p -c usbasp -P usb -U flash:w:test.hex

void rampUp(int from, int to, int step, int delay){
  int i;
  for(i=from; step>0 ? i<to : i>to; i+=step){
    set_motors(i,i);
    delay_ms(delay);
  }
  set_motors(to,to);
}

void turn90(){
  int i;
  for(i=0; i<100; i+=10){
    set_motors(i,-i);
    delay_ms(14);
  }
  delay_ms(40);
  for(i=100; i>0; i-=10){
    set_motors(i,-i);
    delay_ms(14);
  }

//		set_motors(255,-255);
//		delay_ms(70); - at maximum speed wheels sometimes slip
		set_motors(0,0);
}

float sinus[100];
void pendulum(){
  int i;
  for(i=0; i<500; i++ ){
    int v = 40 * sinus[i%100];
    set_motors(v,v);
    delay_ms(100);
  }
  set_motors(0,0);
}
void fillSinus(){
  int i;
  for(i=0;i<100;i++){
    sinus[i] = sin(2*M_PI*i/100);
  }
}

void intermittent(){
int i=0;
for(i=0;i<150;i++){
  //set_motors(90,-90);
  set_motors(90,90);
  delay_ms(40);
  set_motors(0,0);
  delay_ms(40);
}

}

void drunk(){
  
int i=0;
int l=50, r=50;
for(i=0;i<100;i++){
  set_motors(l,r);
  delay_ms(100);
  l += ((random() % 20) - 10)/3;
  r += ((random() % 20) - 10)/3;
}
  set_motors(0,0);

}

void drunk2(){
  
int i=0;
int l,r;
for(i=0;i<4;i++){
#define DRUNK_MEM 15
  char lmem[DRUNK_MEM];   
  char rmem[DRUNK_MEM];
  int j;
  l=50, r=50;
  for(j=0; j<DRUNK_MEM; j++ ){
    l += ((random() % 20) - 10)/3;
    r += ((random() % 20) - 10)/3;
    set_motors(l,r);
    lmem[j]=l;
    rmem[j]=r;
    delay_ms(100);
  }
  for(j=0; j<DRUNK_MEM; j++ ){
    set_motors(rmem[j],lmem[j]);
    delay_ms(100);
  }
}
  set_motors(0,0);

}

void spiral(){
int i;
for( i=0;i<80;i++){
  set_motors(80,i);
  delay_ms(110);
}
for( i=80;i>=0;i--){
  set_motors(80,i);
  delay_ms(70);
}
delay_ms(300);
set_motors(0,0);

}

#define SMOOTH_V 80
void smooth_fwd(int turn){
  int dv2 = turn * SMOOTH_V / 50;
  set_motors(SMOOTH_V - dv2, SMOOTH_V + dv2);
  delay_ms(100);
}
void smooth_bak(int turn){
  int dv2 = turn * SMOOTH_V / 50;
  set_motors(-SMOOTH_V - dv2, -SMOOTH_V + dv2);
  delay_ms(100);
}


#define TURN_MUL 4
#define COURSE_SPD 100
#define MIN_SPD 60
void traj_fwd(int turn, int len){
  if( turn>0 ){
     set_motors(MIN_SPD-35,MIN_SPD+35);
     delay_ms(turn*TURN_MUL);
  }
  if( turn<0 ){
     set_motors(MIN_SPD+35,MIN_SPD-35);
     delay_ms(-turn*TURN_MUL);
  }
  int msMin = len*COURSE_SPD/MIN_SPD/2;
  set_motors(MIN_SPD,MIN_SPD);
  delay_ms(msMin);

  set_motors(COURSE_SPD,COURSE_SPD);
  delay_ms(len*4); // +1 using MIN_SPD = len*5

  set_motors(MIN_SPD,MIN_SPD);
  delay_ms(msMin);
  
}
void traj_bak(int turn, int len){
  if( turn>0 ){
     set_motors(-MIN_SPD-35,-MIN_SPD+35);
     delay_ms(turn*TURN_MUL);
  }
  if( turn<0 ){
     set_motors(-MIN_SPD+35,-MIN_SPD-35);
     delay_ms(-turn*TURN_MUL);
  }
  int msMin = len*COURSE_SPD/MIN_SPD/2;
  set_motors(-MIN_SPD,-MIN_SPD);
  delay_ms(msMin);

  set_motors(-COURSE_SPD,-COURSE_SPD);
  delay_ms(len*4); // +1 using MIN_SPD = len*5

  set_motors(-MIN_SPD,-MIN_SPD);
  delay_ms(msMin);
  
}


void handDrawn(){
traj_fwd( 0 , 142 );
traj_fwd( -16 , 84 );
traj_fwd( -32 , 93 );
traj_fwd( -30 , 94 );
traj_fwd( -51 , 87 );
traj_fwd( -7 , 80 );
traj_fwd( -22 , 68 );
traj_fwd( -14 , 53 );
traj_fwd( -38 , 102 );
traj_fwd( -47 , 105 );
traj_fwd( -20 , 95 );
traj_fwd( -78 , 127 );
traj_fwd( -20 , 75 );
traj_fwd( -69 , 62 );
traj_fwd( -21 , 114 );
traj_fwd( -61 , 62 );
traj_fwd( -41 , 42 );
traj_bak( 0 , 42 );
traj_bak( 42 , 62 );
traj_bak( 61 , 114 );
traj_bak( 21 , 62 );
traj_bak( 69 , 75 );
traj_bak( 20 , 127 );
traj_bak( 77 , 95 );
traj_bak( 20 , 105 );
traj_bak( 47 , 102 );
traj_bak( 38 , 53 );
traj_bak( 15 , 68 );
traj_bak( 22 , 80 );
traj_bak( 7 , 87 );
traj_bak( 51 , 94 );
traj_bak( 30 , 93 );
traj_bak( 32 , 84 );
traj_bak( 16 , 142 );


set_motors(0,0);
}

void smoothHandDrawn(){
smooth_fwd( 0 );
smooth_fwd( 0 );
smooth_fwd( 44 );
smooth_fwd( 54 );
smooth_fwd( 13 );
smooth_fwd( 1 );
smooth_fwd( 9 );
smooth_fwd( 1 );
smooth_fwd( 1 );
smooth_fwd( 1 );
smooth_fwd( 0 );
smooth_fwd( -1 );
smooth_fwd( 4 );
smooth_fwd( -4 );
smooth_fwd( 14 );
smooth_fwd( -5 );
smooth_fwd( 5 );
smooth_fwd( 5 );
smooth_fwd( -5 );
smooth_fwd( -1 );
smooth_fwd( 0 );
smooth_fwd( 1 );
smooth_fwd( -6 );
smooth_fwd( -2 );
smooth_fwd( 4 );
smooth_bak( 19 );
smooth_bak( 2 );
smooth_bak( -2 );
smooth_bak( 3 );
smooth_bak( -1 );
smooth_bak( 0 );
smooth_bak( 0 );
smooth_bak( 0 );
smooth_bak( 3 );
smooth_bak( -2 );
smooth_bak( 2 );
smooth_bak( 2 );
smooth_bak( 0 );
smooth_bak( -2 );
smooth_bak( -5 );
smooth_bak( -2 );
smooth_bak( 1 );
smooth_bak( 1 );
smooth_bak( 9 );
smooth_bak( 4 );
smooth_bak( 87 );
smooth_bak( 34 );
smooth_bak( 2 );
smooth_bak( 3 );
smooth_bak( 1 );
smooth_bak( 1 );
smooth_bak( 2 );
smooth_bak( 4 );
smooth_bak( 2 );
smooth_bak( 0 );
smooth_bak( 0 );
smooth_bak( -2 );
smooth_bak( 1 );
smooth_bak( 1 );
smooth_bak( 10 );
smooth_bak( -4 );
smooth_bak( 2 );
smooth_bak( 1 );
smooth_bak( -5 );
smooth_bak( 4 );
smooth_bak( 1 );
smooth_bak( -6 );
smooth_fwd( 9 );
smooth_fwd( 0 );
smooth_fwd( 2 );
smooth_fwd( 1 );
smooth_fwd( -4 );
smooth_fwd( 2 );
smooth_fwd( 5 );
smooth_fwd( -3 );
smooth_fwd( 2 );
smooth_fwd( -2 );
smooth_fwd( 2 );
smooth_fwd( 1 );
smooth_fwd( -6 );
smooth_fwd( 5 );
smooth_fwd( 0 );
smooth_fwd( 0 );
smooth_fwd( 0 );
smooth_fwd( 10 );
smooth_fwd( 2 );
smooth_fwd( 1 );
smooth_fwd( 5 );
smooth_fwd( 59 );
smooth_fwd( 39 );
smooth_fwd( 16 );
smooth_fwd( 1 );
smooth_fwd( 4 );
smooth_fwd( 4 );
smooth_fwd( 2 );
smooth_fwd( 4 );
smooth_bak( 0 );
smooth_bak( -4 );
smooth_bak( -2 );
smooth_bak( -4 );
smooth_bak( -4 );
smooth_bak( -1 );
smooth_bak( -16 );
smooth_bak( -39 );
smooth_bak( -59 );
smooth_bak( -5 );
smooth_bak( -1 );
smooth_bak( -2 );
smooth_bak( -11 );
smooth_bak( 0 );
smooth_bak( 0 );
smooth_bak( 0 );
smooth_bak( -5 );
smooth_bak( 6 );
smooth_bak( -1 );
smooth_bak( -2 );
smooth_bak( 2 );
smooth_bak( -2 );
smooth_bak( 3 );
smooth_bak( -5 );
smooth_bak( -2 );
smooth_bak( 4 );
smooth_bak( -1 );
smooth_bak( -2 );
smooth_bak( 0 );
smooth_fwd( -8 );
smooth_fwd( 6 );
smooth_fwd( -1 );
smooth_fwd( -4 );
smooth_fwd( 5 );
smooth_fwd( -1 );
smooth_fwd( -2 );
smooth_fwd( 4 );
smooth_fwd( -10 );
smooth_fwd( -1 );
smooth_fwd( -1 );
smooth_fwd( 2 );
smooth_fwd( 0 );
smooth_fwd( 0 );
smooth_fwd( -2 );
smooth_fwd( -4 );
smooth_fwd( -2 );
smooth_fwd( -1 );
smooth_fwd( -1 );
smooth_fwd( -3 );
smooth_fwd( -2 );
smooth_fwd( -34 );
smooth_fwd( -87 );
smooth_fwd( -4 );
smooth_fwd( -9 );
smooth_fwd( -1 );
smooth_fwd( -1 );
smooth_fwd( 2 );
smooth_fwd( 5 );
smooth_fwd( 2 );
smooth_fwd( 0 );
smooth_fwd( -2 );
smooth_fwd( -2 );
smooth_fwd( 2 );
smooth_fwd( -3 );
smooth_fwd( 0 );
smooth_fwd( 0 );
smooth_fwd( 0 );
smooth_fwd( 1 );
smooth_fwd( -3 );
smooth_fwd( 2 );
smooth_fwd( -2 );
smooth_bak( -19 );
smooth_bak( -4 );
smooth_bak( 2 );
smooth_bak( 6 );
smooth_bak( -1 );
smooth_bak( 0 );
smooth_bak( 1 );
smooth_bak( 5 );
smooth_bak( -5 );
smooth_bak( -5 );
smooth_bak( 5 );
smooth_bak( -14 );
smooth_bak( 4 );
smooth_bak( -4 );
smooth_bak( 1 );
smooth_bak( 0 );
smooth_bak( -1 );
smooth_bak( -1 );
smooth_bak( -1 );
smooth_bak( -9 );
smooth_bak( -1 );
smooth_bak( -13 );
smooth_bak( -54 );
smooth_bak( -44 );
smooth_bak( 0 );
set_motors(0,0);
}

void calibrateSensors(char c, unsigned int sen[5]){
  do{
        read_line_sensors(sen,IR_EMITTERS_ON);
	clear();                  // clear the LCD
	lcd_goto_xy(0, 0);
        int i;
        print_character(c);
        for( i=0; i<2; i++ ){
          print_character(' ');
	  print_long(sen[i]/16);
        }
        lcd_goto_xy(0, 1);
        print_long(sen[2]/16);
        print_character(' ');
        print_long(sen[3]/16);
        print_character(' ');
        print_long(sen[4]/16);
	delay_ms(100);
  }while( ! button_is_pressed(BUTTON_B) );
  while(button_is_pressed(BUTTON_B));
  play_frequency(6000, 150, 15);
}

void printSensorDiff(unsigned int senIn[5], unsigned int senOut[5]){
        clear(); 
	lcd_goto_xy(0, 0);
        int i;
        print_character('D');
        for( i=0; i<2; i++ ){
          print_character(' ');
	  print_long(((int)senOut[i]-(int)senIn[i])/16);
        }
        lcd_goto_xy(0, 1);
        print_long(((int)senOut[2]-(int)senIn[2])/16);
        print_character(' ');
        print_long(((int)senOut[3]-(int)senIn[3])/16);
        print_character(' ');
        print_long(((int)senOut[4]-(int)senIn[4])/16);
}

void checkOutsideVisualize(unsigned int senIn[5], unsigned int senOut[5], unsigned int sen[5]){
        clear(); 
	lcd_goto_xy(0, 0);
	

	//unsigned int sen[5];
        //read_line_sensors(sen,IR_EMITTERS_ON);
	int i;
	for( i=0; i<5; i++ ){
		// senIn is less than senOut
		int max = ((int)senOut[i]+(int)senIn[i])/2;
		if( sen[i] >= max ){
			        print_character('*');
		}else{
			        print_character('_');
		}
 	}
}

int checkOutside(unsigned int senIn[5], unsigned int senOut[5]){
	unsigned int sen[5];
        read_line_sensors(sen,IR_EMITTERS_ON);
	int i;
	for( i=0; i<5; i++ ){
		// senIn is less than senOut
		int max = ((int)senOut[i]+(int)senIn[i])/2;
		if( sen[i] >= max ){
                        checkOutsideVisualize(senIn, senOut, sen);
			return 1;
		}
 	}
	return 0;
}


void sensors(){
pololu_3pi_init(1500);
  unsigned int senIn[5]; // an array to hold sensor values
  unsigned int senOut[5]; // an array to hold sensor values
  
  calibrateSensors('I',senIn);
  calibrateSensors('O',senOut);
  printSensorDiff(senIn, senOut);

  while(!button_is_pressed(BUTTON_B));
  while(button_is_pressed(BUTTON_B));
  play_frequency(6000, 150, 15);
  delay_ms(1000);

  set_motors(40,40);
  int outside=0;
  while(outside<2){
    if( checkOutside(senIn, senOut)==0 ){
      outside=0;
    }else{
      outside++;
    }
  }
  
  set_motors(0,0);

  while(!button_is_pressed(BUTTON_B));
  while(button_is_pressed(BUTTON_B));
  for(;;){
    unsigned int sen[5];
    read_line_sensors(sen,IR_EMITTERS_ON);
    checkOutsideVisualize(senIn, senOut, sen);
    delay_ms(300);
  }

  for(;;);
}

int main()
{
	clear();                  // clear the LCD
	lcd_goto_xy(0, 0);
	print("BatteRY");

	int bat = read_battery_millivolts();

	lcd_goto_xy(0, 1);
	print_long(bat);
	print("mV");

	//fillSinus();
	
	// wait for all buttons to be released, then a press
	// ANY_BUTTON
	while(button_is_pressed(BUTTON_B));
	/*char button =*/ wait_for_button_press(BUTTON_B);
long seed=0;
	while(button_is_pressed(BUTTON_B)) seed++;
srandom(seed);
       // delay_ms(1000);

sensors();
//intermittent();
//drunk2();
//spiral();
//handDrawn();
//smoothHandDrawn();
       /* 
	
	set_motors(255,255);
        delay_ms(1000);
        rampUp(255,0,-25,20);
	delay_ms(1000);
        rampUp(0,-255,-25,20);
        delay_ms(1000);
	set_motors(0,0);
*/
	//float aa=sin(1);
	//lcd_goto_xy(0, 0);
	//print_long((int)(aa*100));

	//pendulum();
	return 0;

	//if(button & BUTTON_A)

        int i;
        for( i=10; i<255; i+=10 ){
//	  	clear();                  // clear the LCD/
//		lcd_goto_xy(0, 0);
//		print("i=");
//		print_long(i);

		rampUp(0,55,20,30);
		delay_ms(1200);
		rampUp(55,0,-20,30);
		delay_ms(200);

		turn90();
		delay_ms(200);
       }

	clear();                  // clear the LCD
	lcd_goto_xy(0, 0);
	print("Reset me");

	return 0;
}


// Local Variables: **
// mode: C **
// c-basic-offset: 4 **
// tab-width: 4 **
// indent-tabs-mode: t **
// end: **
